import math
import heapq
import numpy as np


def point_to_sphere_distance(point, sphere_center, radius):
    """
    计算点到球心的距离，并判断点是否在球内。

    参数：
    point (tuple): 点的坐标 (x, y, z)。
    sphere_center (tuple): 球心的坐标 (cx, cy, cz)。
    radius (float): 球的半径。

    返回：
    dict: 包含点到球心的距离和是否在球内的判断。
    """
    # 解包坐标
    x, y, z = point
    cx, cy, cz = sphere_center

    # 计算点到球心的距离
    distance = math.sqrt((x - cx) ** 2 + (y - cy) ** 2 + (z - cz) ** 2)

    # 判断点是否在球内（包括边界）
    is_inside = distance <= radius

    return {
        "distance": distance,
        "is_inside": is_inside
    }


def find_closest_points_heap_3d(points, new_point, k):
    """
    使用堆找到离新点最近的 k 个点（三维空间）

    :param points: 已知点的列表 [(x1, y1, z1), (x2, y2, z2), ...]
    :param new_point: 新点的坐标 (x, y, z)
    :param k: 需要找到的最近点数量
    :return: 离新点最近的 k 个点及其距离
    """
    max_heap = []

    for p in points:
        d = math.sqrt(
            (p[0] - new_point[0]) ** 2 +
            (p[1] - new_point[1]) ** 2 +
            (p[2] - new_point[2]) ** 2
        )
        # 使用负距离构建最大堆
        heapq.heappush(max_heap, (-d, p))
        if len(max_heap) > k:
            heapq.heappop(max_heap)  # 移除堆中最远的点

    # 取出堆中元素并还原距离
    closest_p = [(-d, p) for d, p in max_heap]
    return sorted(closest_p, key=lambda x: x[0])


def triangle_centroid(vertices):
    """
    计算三角形的几何中心（质心）

    :param vertices: 三角形三个顶点的坐标 [(x1, y1, z1), (x2, y2, z2), (x3, y3, z3)]
    :return: 几何中心坐标 (xc, yc, zc)
    """
    x_c = sum(vertex[0] for vertex in vertices) / 3
    y_c = sum(vertex[1] for vertex in vertices) / 3
    z_c = sum(vertex[2] for vertex in vertices) / 3
    return x_c, y_c, z_c


def quadrilateral_centroid(vertices):
    """
    计算凸四边形的几何中心（质心）

    :param vertices: 四边形四个顶点的坐标 [(x1, y1, z1), (x2, y2, z2), (x3, y3, z3), (x4, y4, z4)]
    :return: 几何中心坐标 (xc, yc, zc)
    """
    x_c = sum(vertex[0] for vertex in vertices) / 4
    y_c = sum(vertex[1] for vertex in vertices) / 4
    z_c = sum(vertex[2] for vertex in vertices) / 4
    return x_c, y_c, z_c


def cal_point_to_line(vertices_c, vertices_q, vertices_p):
    """
    计算点C到点Q和点P所在直线的距离

    :param vertices_c 点C
    :param vertices_q 点Q
    :param vertices_p 点P
    :return 点到直线距离 direct_distance
    """
    # 解包坐标
    x_c, y_c, z_c = vertices_c
    x_q, y_q, z_q = vertices_q
    x_p, y_p, z_p = vertices_p

    vector_a = (x_c - x_q, y_c - y_q, z_c - z_q)
    vector_b = (x_p - x_q, y_p - y_q, z_p - z_q)

    x_a, y_a, z_a = vector_a
    x_b, y_b, z_b = vector_b

    distance = (x_a ** 2 + y_a ** 2 + z_a ** 2) - ((x_a * x_b + y_a * y_b + z_a * z_b) ** 2) / (x_b ** 2 + y_b ** 2 + z_b ** 2)
    direct_distance = math.sqrt(distance)

    return direct_distance


def find_perpendicular_point(point_A, point_B, point_C, point_D):
    """
    计算点A到点B、点C、点D所在平面的垂足点P

    :param point_A 点A
    :param point_B 点B
    :param point_C 点C
    :param point_D 点D
    :return 垂足点P point_P
    """
    # 向量 BC 和 BD
    vector_BC = point_C - point_B
    vector_BD = point_D - point_B

    # 计算法向量 n = BC x BD
    n = np.cross(vector_BC, vector_BD)

    # 计算 t 的值
    t = np.dot(n, point_B - point_A) / np.dot(n, n)

    # 计算垂足 P
    point_P = point_A + t * n

    return point_P


def is_point_in_triangle(point_P, point_B, point_C, point_D):
    """
    判断点P是否在点B、点C、点D所在平面内

    :param point_P 点P
    :param point_B 点B
    :param point_C 点C
    :param point_D 点D
    :return 判断结果 True/False
    """
    # 计算向量 BC, CD, DB
    vector_BC = point_C - point_B
    vector_CD = point_D - point_C
    vector_DB = point_B - point_D

    # 计算向量 P-B, P-C, P-D
    vector_PB = point_P - point_B
    vector_PC = point_P - point_C
    vector_PD = point_P - point_D

    # 计算叉积
    cross1 = np.cross(vector_BC, vector_PB)
    cross2 = np.cross(vector_CD, vector_PC)
    cross3 = np.cross(vector_DB, vector_PD)
    print("cross1", cross1)
    print("cross2", cross2)
    print("cross3", cross3)

    # 判断叉积是否同向（同侧）并且考虑边界情况
    def is_same_side(cross1, cross2):
        return np.dot(cross1, cross2) >= 0

    # 对于边界上的点，可以通过判断叉积为零并且点在边的范围内来处理
    if is_same_side(cross1, cross2) and is_same_side(cross2, cross3) and is_same_side(cross3, cross1):
        return True
    return False


if __name__ == '__main__':
    # 示例1
    point = (1, 2, 3)
    sphere_center = (4, 5, 6)
    radius = 6
    distance = point_to_sphere_distance(point, sphere_center, radius)
    print(f"点到球心的距离为: {distance}")

    # 示例2
    points_3d = [
        (1, 2, 3), (4, 5, 6), (7, 8, 9), (2, 1, 0),
        (3, 3, 3), (6, 5, 4), (8, 9, 7), (0, 0, 0),
        (10, 10, 10), (1, 1, 1), (9, 8, 7), (4, 3, 2)
    ]
    new_point_3d = (0, 1, 3)
    k = 4
    closest_points = find_closest_points_heap_3d(points_3d, new_point_3d, k)
    print(f"使用堆离新点最近的 {k} 个点：")
    for distance, point in closest_points:
        print(f"点 {point} 到新点 {new_point_3d} 的距离: {distance:.2f}")

    # 示例3
    triangle = [(1, 1, 1), (4, 1, 4), (2, 3, 6)]
    centroid = triangle_centroid(triangle)
    print(f"三角形的几何中心：{centroid}")

    # 示例4
    quadrilateral = [(0, 0, 0), (4, 0, 0), (4, 3, 1), (0, 3, 0)]
    centroid = quadrilateral_centroid(quadrilateral)
    print(f"四边形的几何中心：{centroid}")

    # 示例5
    vertices_c = (-2, -1, 0)
    vertices_q = (0, 0, 0)
    vertices_p = (-6, 0, 0)
    distance = cal_point_to_line(vertices_c, vertices_q, vertices_p)
    print(f"点到直线的距离是：{distance}")

    # 示例6
    A = np.array([1/2, 1/2, 0])  # 点 A 的坐标
    B = np.array([0, 0, 0])  # 点 B 的坐标
    C = np.array([1, 0, 0])  # 点 C 的坐标
    D = np.array([0, 1, 0])  # 点 D 的坐标
    # 计算点 A 到三角面的垂足
    P = find_perpendicular_point(A, B, C, D)
    print("点 A 到三角面的垂足 P:", P)
    # 判断垂足是否在三角形内部
    is_inside = is_point_in_triangle(P, B, C, D)
    print("垂足 P 是否在三角形内部:", is_inside)

    """
    球心位置改变需要全量重新计算点到球心距离
    是否需要保留已选择状态
    
    1. 全包，计算三个点或者四个点里球心距离是否都小于等于球体半径
    1.1 通过堆计算所有点到球心的距离，将所有点按到球心距离排序，再建立面和点的关系，判断形成面的点是否都在球体半径内
    1.2 是否有方法可以先判断球最初和配件接触的点，即计算距离球体最近的点A，以点A为起始向周围辐射，按距离远近建立二叉树/KD树，通过深度遍历判断其周围的点到球心的距离是否小于球体半径
    2. 相交，计算球心到三角面的垂足，判断垂足是否在三角面内部。（四边形转化为两个三角形计算）
    3. 几何中心，先计算几何中心，然后判断几何中心到球心的距离是否小于等于球体半径
    """
